#ifndef _CANOPEN_H__
#define _CANOPEN_H__

#include "rclcpp/rclcpp.hpp"
#include <socketcan_interface/socketcan.h>
#include <vector>
#include <thread>
#include <mutex>
#include <queue>

#define SLAVE_HEARTBEAT_CYCLE (1000)

#define SDO_W_FUNC (0xAA)
#define SDO_R_FUNC (0xBB)

#define CAN_DATA_LENGTH (8)
#define CANOPEN_NODES (4) //支持的节点数量
#define PER_NODE_PDO_NUM (4) //每个节点支持的r/tpdo个数
#define PER_PDO_OBJ_NUM (8) //pdo映射的对象字典个数
#define COMM_SDO_FIFO_MAX (100) //SDO发送缓存队列长度
#define COMM_PDO_SUM (CANOPEN_NODES*PER_NODE_PDO_NUM)
#define COMM_PDO_QUEUE_SIZE (CANOPEN_NODES*PER_NODE_PDO_NUM)
#define COMM_PDO_PERIOD_TRANSMISSION (10)

#define NMT_OPERATION (0x01)
#define NMT_STOP (0x02)
#define NMT_PRE_OPERATION (0x80)
#define NMT_RESET_APP (0x81)
#define NMT_RESET_COMM (0x82)

typedef enum
{
    /* kinco */
    E_RPDO_60FF00 = 0, // target speed
    E_RPDO_607A00, // pos value
    E_RPDO_604000, //ctl word
    /*curtis user-defined*/
    E_RPDO_300000, // ctl word
    E_RPDO_300001, // speed 50~4000 unit=rpm
    E_RPDO_300002, // acc 2~255 unit=0.1s
    E_RPDO_300003, // dec 2~255 unit=0.1s
    E_RPDO_300004, // steer angle -12000~12000 unit is 0.01degree
    E_RPDO_300005, // forkLift descend speed 0~200

    E_RPDO_300100, // unused 2byte
    E_RPDO_300101, // direct forkLift speed
    E_RPDO_300102, // alternating forkLift speed
    E_RPDO_300103, // alternating forkLift acc
    E_RPDO_300104, // alternating forkLift dec
    E_RPDO_300105, // unused
    /* zapi */
    E_RPDO_200100, // 
    E_RPDO_200200,
    E_RPDO_200300,
    E_RPDO_BUTT,
}CANOPEN_RPDO_E;

typedef struct COMM_SDODATA_S
{
    uint8_t nodeId;
    uint8_t rwFlag;
    uint16_t len;
    uint16_t index;
    uint8_t subIndex;
    int32_t data;
}COMM_SDODATA;

typedef struct COMM_PDODATA_S
{
    uint8_t nodeId;
    uint8_t pdoSer;
    uint8_t len;
    uint8_t data[CAN_DATA_LENGTH];
}COMM_PDODATA;

typedef struct COMM_NMTDATA_S
{
    uint8_t nodeId;
    uint8_t data;
}COMM_NMTDATA,*pCOMM_NMTDATA;

typedef struct COMM_PDOMAPCFG_S
{
    uint16_t period;
    uint16_t pdoSer;
    uint16_t index[PER_PDO_OBJ_NUM];
    uint8_t subIndex[PER_PDO_OBJ_NUM];
    uint8_t startByte[PER_PDO_OBJ_NUM];
    uint8_t len[PER_PDO_OBJ_NUM];
}COMM_PDOMAPCFG,*pCOMM_PDOMAPCFG;

typedef struct COMM_PDO_CFG_S
{
    uint8_t nodeId;
    COMM_PDOMAPCFG rPdoMapCfg[PER_NODE_PDO_NUM];
    COMM_PDOMAPCFG tPdoMapCfg[PER_NODE_PDO_NUM];
    int rpdoNum;
    bool sdoToPdoMap;
}COMM_PDO_CFG,*pCOMM_PDO_CFG;

typedef struct COMM_TPDODATA_LIST_S
{
    /* kinco */
    int pdo606300; //feedback position
    int pdo606c00; //feedback velocity
    uint16_t pdo604100; //status word
    uint16_t pdo603f00; //error code
    uint32_t pdo608100; //position velocity
    uint32_t pdo60FD00; //digital input 
    int8_t pdo606100; //actual mode
    /* zapi */
    int16_t pdo210100;
    uint16_t pdo210200;
    /* curtis */
    int16_t pdo310000; //feedback velocity unit=rpm
    uint16_t pdo310001; //feedback current unit=0.1A
    uint8_t pdo310002; //error code
    uint8_t pdo310003; //battery level 0~100
    int16_t pdo310004; //temperature uint=0.1
    int32_t pdo310100; //odometer uint=cm
    uint32_t pdo310101; //unused
    int16_t pdo310200; //feedback angle unit=0.01degree
    uint8_t pdo310201; //steer errcode

    /* TOFI cableEncoder*/
    uint16_t pdo320000; //pos uint=mm
    uint16_t pdo320001; //unknown
    uint16_t pdo320002; // vel uint=0.1mm/s
}COMM_TPDODATA_LIST,*pCOMM_TPDODATA_LIST;

typedef struct COMM_TSDODATA_LIST_S
{
    uint16_t sdo604100;
    uint8_t sdo606100;
}COMM_TSDODATA_LIST,*pCOMM_TSDODATA_LIST;

typedef struct COMM_TNMTDATA_LIST_S
{
    uint8_t nodeId;
    uint8_t nmtData;
    rclcpp::Time nmtTime;
}COMM_TNMTDATA_LIST,*pCOMM_TNMTDATA_LIST;

typedef struct COMM_NODEGUARDING_CFG_S
{
    int nodeGuarding_mode;
    uint16_t nodeGuarding_time;
    uint8_t coefficient_of_time;
}COMM_NODEGUARDING_CFG,*pCOMM_NODEGUARDING_CFG;

typedef struct COMM_RPDOMAPCTL_S
{
    uint8_t pdoSer;
    uint8_t startByte;
    uint8_t len;
}COMM_RPDOMAPCTL,*pCANOPEN_RPDOMAPCTL;

typedef struct COMM_RPDOCTL_S
{
    COMM_RPDOMAPCTL rPdoMapCtl[E_RPDO_BUTT];
}COMM_RPDOCTL,*pCOMM_RPDOCTL;

typedef struct COMM_RPDOQUEUE_S
{
    uint8_t num; //RPDO个数
    uint32_t period; //pdo周期
    COMM_PDODATA buf[COMM_PDO_SUM];
}COMM_RPDOQUEUE,*pCOMM_RPDOQUEUE;

typedef struct COMM_PDOCTL_S
{
    COMM_RPDOCTL rPdoCtl[CANOPEN_NODES];
    COMM_RPDOQUEUE rpdQueue;
    uint8_t RpdoQueueMap[COMM_PDO_QUEUE_SIZE][2]; //记录每个pdo对应的pdo队列序号
    uint8_t RpdoQueueIdxMax;
    bool pdoStartFlag; //pdo发送开启标志
}COMM_PDOCTL,*pCOMM_PDOCTL;


typedef struct COMM_NODEPROTECTION_S
{
    uint16_t nodeProtectionPeriod[CANOPEN_NODES];
    rclcpp::Time nodePreTime[CANOPEN_NODES];
}COMM_NODEPROTECTION,*pCOMM_NODEPROTECTION;

typedef struct COMM_PORT_S
{
    COMM_PDO_CFG cfg[CANOPEN_NODES];
    COMM_TPDODATA_LIST tPdoList[CANOPEN_NODES];
    COMM_TSDODATA_LIST tSdoList[CANOPEN_NODES];
    COMM_TNMTDATA_LIST tNmtList[CANOPEN_NODES];
    COMM_NODEGUARDING_CFG nodeGuardingCfg[CANOPEN_NODES];
    COMM_PDOCTL ctl;
    COMM_NODEPROTECTION nodeProCtl;
}COMM_PORT,*pCOMM_PORT;

class Canopen
{
private:
    can::DriverInterfaceSharedPtr driver_;
    can::FrameListenerConstSharedPtr frame_listener_;

    uint8_t initStep;

    std::thread m_thread;
    std::timed_mutex  m_quit_mutex;
    std::thread sdoSendThread;
    std::timed_mutex  m_sdoSend_quit_mutex;

    int nodeId;
    int interval_ms;
    COMM_PORT m_commPort;
    
    rclcpp::TimerBase::SharedPtr synctimer;
    static Canopen *instancePtr;
    std::queue<can::Frame> sdoSendQueue;
    std::shared_ptr<rclcpp::Node> node_;
public:
    explicit Canopen(std::shared_ptr<rclcpp::Node> node);
    ~Canopen();
    enum State{
        Unknown = 255, BootUp = 0, Stopped = 4, Operational = 5 , PreOperational = 127
    };

    enum nodeGuardingMode{
        Nouse = 0, NodeGuarding = 1, Heartbeat = 2
    };
    
    std::shared_ptr<rclcpp::Node> GetNode() {
        return node_;
    }

    bool driver_init(void);

    void paramInit();

    int canopen_init(uint8_t nodeId);

    void nmtSend(uint8_t node, uint8_t cmd);

    void syncSend();

    void sdoSend(uint8_t node, uint8_t rwflag, uint8_t len, uint16_t index, uint8_t subIndex, uint32_t data);

    void sdoPopSend();

    int pdoSend(uint8_t nodeId, CANOPEN_RPDO_E index, int32_t data);

    void canmsg_cb(const can::Frame& msg);

    void comm_PdoStart(bool startFlag);

    int commPdoRunTime(void);

    void thread_runtime();

    void nodeProtectionRuntime();

    static void thread_fun(Canopen *pThis);

    static void sdoSendFunc(Canopen *pThis);

    int nodeIdToIdx(uint8_t nodeId);

    int canopenRpdoToQueueIdx(uint8_t nodeId, uint8_t pdoSer);

    int RpdoDataUpdate(uint8_t ser,uint8_t nodeId, uint8_t pdoSer, uint8_t startByte, uint8_t len, int32_t data);

    int commTsdoAnalyse(COMM_SDODATA sdoData);

    int commTpdoAnalyse(COMM_PDODATA pdoData);

    int commTnmtAnalyse(COMM_NMTDATA nmtData);

    void emcy_handle(const can::Frame& msg);

    int canopenPdoMapSend(uint8_t nodeId, uint8_t trPdoFlag, COMM_PDOMAPCFG pdoCfg);

    int canopenPdoMap(uint8_t nodeId);

    int canopenRpdoDataMapInit(uint8_t nodeId);

    int canopenRpdoDataMap(uint8_t ser, uint8_t pdoSer, uint16_t index, uint8_t subIndex, uint8_t startByte, uint8_t len);

    int canopenRpdoQueueIdxMap(uint8_t nodeId, uint8_t pdoSer);

    int canopenNodeGuarding(uint8_t nodeId);

    uint16_t getStatusWord(uint8_t nodeId);

    int getEnc(uint8_t nodeId);

    uint32_t getPvel(uint8_t nodeId);

    int getVvel(uint8_t nodeId, int type);

    uint16_t getErrCode(uint8_t nodeId);

    uint32_t getDiStatus(uint8_t nodeId, int type);

    int8_t getActualCtlMode(uint8_t nodeId);

    void nodeGuarding_start(uint8_t nodeId, uint16_t period);

    void nodeGuarding_send(uint8_t nodeId);

    void checkNodeStatus();

    static Canopen *getInstance(std::shared_ptr<rclcpp::Node> node) {
        // If there is no instance of class
        // then we can create an instance.
        if(instancePtr == NULL)
        {
            // We can access private members
            // within the class.
            instancePtr = new Canopen(node);
            
            // returning the instance pointer.
            return instancePtr;
        } else {  
            // if instancePtr != NULL that means
            // the class already has an instance.
            // So, we are returning that instance
            // and not creating new one.
            return instancePtr;
        } 
    }

    int16_t getAngle(uint8_t nodeId);
    uint32_t getOdomter(uint8_t nodeId);
    uint32_t get_cableEncoderPos(uint8_t nodeId, int type);
};


#endif
